#ifndef motor_H
#define motor_H

#include <ros/ros.h>

#include <driver/Move.h>
#include <driver/Movement.h>

#include <robo/Encoder.h>
#include <robo/Motor.h>

class Motor
{

public:
	Motor();

	void receiveDirections(const driver::Move::ConstPtr &msg);
	void receiveEncoder(const robo::Encoder::ConstPtr &msg);

	void updateOdometry(float diff_time, int diff_left_ticks, int diff_right_ticks);
	void publishMovement(float dist_left, float dist_right);
private:
	bool simulation_;
	bool rotating_;
	bool rotate_only_;
	bool wait_until_completion_;

	float current_angle_;
	float current_x_;
	float current_y_;
	float current_speed_;

	float target_angle_;
	float target_x_;
	float target_y_;

	double prev_timestamp_;
	int64_t prev_left_;
	int64_t prev_right_;

	double last_message_timestamp_;

	float rotate_start_angle_;
	int64_t rotate_start_left_ticks_;
	int64_t rotate_start_right_ticks_;
	int64_t rotate_end_left_ticks_;
	int64_t rotate_end_right_ticks_;

	// Measurements
	double encoder_ticks_;
	double min_speed_;
	double max_speed_;
	double wheel_base_;
	double wheel_radius_;
	double rotation_accuracy_;
	double distance_accuracy_;
	double max_acceleration_;
	
	double distance_per_tick_;

	// Subscribers and publishers
	ros::Subscriber	enc_sub;
	ros::Subscriber mov_sub;
	ros::Publisher	mot_pub;
	ros::Publisher	int_pub;
	ros::Publisher  mov_pub;
};

#endif
